#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <omp.h>

struct pointer {
	struct node *ptr;
	int count;
};

struct node {
	struct pointer *next;
	void *value;
};

struct queue {
	struct pointer *head;
	struct pointer *tail;
};

void initialize( struct queue *queue) {
	struct node *node;
	struct pointer *next;
	struct pointer *headpointer;
	struct pointer *tailpointer;

	// prepare node
	node = malloc( sizeof(struct node) );
	next = malloc( sizeof(struct pointer) );
	node->value = NULL;
	node->next = next;
	next->ptr = NULL;
	next->count = 0;

	headpointer = malloc( sizeof(struct pointer) );
	tailpointer = malloc( sizeof(struct pointer) );
	headpointer->ptr = node;
	tailpointer->ptr = node;
	headpointer->count = 0;
	tailpointer->count = 0;
	queue->head = headpointer;
	queue->tail = tailpointer;
}


// append data to q->tail
void enqueue( struct queue *queue, void *value ) {
	struct node *node;
	struct pointer *nxt;
	struct pointer *tail;
	struct pointer *next;
	struct pointer *newnext;
	struct pointer *newtail;
	int shouldBreak = 0;

	// prepare new node
	node = malloc( sizeof(struct node) );
	nxt = malloc( sizeof(struct pointer) );
	node->value = value;
	node->next = nxt;
	nxt->ptr = NULL;
	nxt->count = 0;


	while(1) {
		tail = queue->tail;
		next = tail->ptr->next;
		if (tail == queue->tail) {
			if (next->ptr == NULL) {
				//atomic (replace nextPointer)
				#pragma omp critical 
				{
					if (tail->ptr->next == next) {
						shouldBreak = 1;
						newnext = malloc( sizeof(struct pointer) );
						newnext->ptr = node;
						newnext->count = next->count+1;
						tail->ptr->next = newnext;
						free(next);
					}
				}
				if (shouldBreak) {
					break;
				}
			}
			else {
				#pragma omp critical 
				{
					//atomic (replace tailPointer)
					if (queue->tail == tail) {
						newtail = malloc( sizeof(struct pointer) );
						newtail->ptr = next->ptr;
						newtail->count = tail->count+1;
						queue->tail = newtail;
						free(tail);
					}
				}
			}
		}
	}
	#pragma omp critical 
	{
		//atomic (replace tailPointer)
		if (queue->tail == tail) {
			newtail = malloc( sizeof(struct pointer) );
			newtail->ptr = tail->ptr->next->ptr;
			newtail->count = tail->count+1;
			queue->tail = newtail;
			free(tail);
		}
	}
}

// return q->head->data
void *dequeue( struct queue *queue) {
	void *pvalue;
	struct pointer *head;
	struct pointer *tail;
	struct pointer *next;
	struct pointer *newtail;
	struct pointer *newhead;
	int shouldBreak = 0;

	while(1) {
		head = queue->head;
		tail = queue->tail;
		next = head->ptr->next;
		if (head == queue->head) {
			if (head->ptr == tail->ptr) {
				if (next->ptr == NULL) {
					return NULL;
				}
				#pragma omp critical 
				{
					//atomic (replace tailPointer)
					if (queue->tail == tail) {
						newtail = malloc( sizeof(struct pointer) );
						newtail->ptr = next->ptr;
						newtail->count = tail->count+1;
						queue->tail = newtail;
						free(tail);
					}
				}
			}
			else {
				pvalue = next->ptr->value;
				#pragma omp critical 
				{
					//atomic (replace headPointer)
					if (queue->head == head) {
						shouldBreak = 1;
						newhead = malloc( sizeof(struct pointer) );
						newhead->ptr = next->ptr;
						newhead->count = head->count+1;
						queue->head = newhead;
					}
				}
				if (shouldBreak) {
					break;
				}
			}
		}
	}
	free(head->ptr);
	free(head);
	return pvalue;
}


int main( int argc, char **argv ) {
	struct queue *q;
	q = malloc( sizeof(struct queue) );

	initialize(q);

	#pragma omp parallel 
	#pragma omp single nowait 
	{
		long i=0;
		for(i=1;i<3;++i) {
			#pragma omp task 
			{
				long j=0;
				for(j=0;j<20;++j) {
					enqueue(q,(void*)(i*1000+j));
				}
			}
			#pragma omp task 
			{
				long d;
				long j=0;
				for(j=0;j<20;++j) {
					d = (long)dequeue(q);
					if(d)
						printf("dequeue %ld\n",d);
				}
			}
		}
	}

	long d;
	while(1) {
		d = (long)dequeue(q);
		if(!d)
			break;
		printf("dequeue outside %ld\n",d);
	}
	assert(q->head->ptr == q->tail->ptr);
	return 0;
}
